import math


def getting_tracking_poly_points(path_point_i_x, path_point_i_y, path_point_i_angle_rad, latlon_CAR_WIDTH):

    heading_rad = path_point_i_angle_rad
    offset = latlon_CAR_WIDTH/2
    # 计算垂直车身的方向角(正北基准)
    left_angle = heading_rad + math.pi/2  # 左侧垂直方向
    right_angle = heading_rad - math.pi/2  # 右侧垂直方向
    dx_left = offset * math.sin(left_angle)  # 东向偏移
    dy_left = offset * math.cos(left_angle)  # 北向偏移
    dx_right = offset * math.sin(right_angle)
    dy_right = offset * math.cos(right_angle)

    left_point_x = path_point_i_x + dx_left
    left_point_y = path_point_i_y + dy_left
    right_point_x = path_point_i_x + dx_right
    right_point_y = path_point_i_y + dy_right

    return left_point_x,left_point_y,right_point_x,right_point_y